#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
舵机控制通信协议

数据帧格式说明：
+------------+---------------+----------------+----------+------------+
| 字节位置    | 字段名称      | 描述           | 长度(字节) | 示例值      |
+------------+---------------+----------------+----------+------------+
| 0 ~ 1      | 帧头          | 固定值         | 2        | AA BB      |
| 2          | 舵机数量      | 控制的舵机数量  | 1        | 02         |
| 3 ~ 29     | 舵机数据      | 9组舵机数据    | 27       | 见下文      |
| 30         | 校验和        | 前面所有数据的和| 1        | F1         |
| 31 ~ 32    | 帧尾          | 固定值         | 2        | CC DD      |
+------------+---------------+----------------+----------+------------+

舵机数据格式(每组3字节)
+------------+---------------+----------------+----------+------------+
| 字节位置    | 字段名称      | 描述           | 长度(字节) | 示例值      |
+------------+---------------+----------------+----------+------------+
| 0          | 舵机引脚号    | 对应Arduino引脚 | 1        | 04         |
| 1 ~ 2      | 舵机角度      | 放大10倍的角度值| 2        | 03 84      |
+------------+---------------+----------------+----------+------------+

示例：
AA BB 01 00 00 00 00 00 00 04 03 84 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 F1 CC DD

含义解析：
- 帧头: AA BB
- 舵机数量: 01(控制1个舵机)
- 舵机数据:
  - 引脚2: 00 00 00(未使用）
  - 引脚3: 00 00 00(未使用）
  - 引脚4: 04 03 84(控制4号引脚舵机, 角度为0x0384=900, 即90.0度）
  - 引脚6: 00 00 00(未使用)
  - 引脚44: 00 00 00(未使用)
  - 引脚45: 00 00 00(未使用)
  - 引脚46: 00 00 00(未使用)
  - 引脚7: 00 00 00(未使用)
  - 引脚10: 00 00 00(未使用)
- 校验和: F1(所有前面数据的和与0xFF)
- 帧尾: CC DD
"""

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

class ServoProtocol:
    FRAME_HEADER = bytes([0xAA, 0xBB])  # 帧头
    FRAME_TAIL = bytes([0xCC, 0xDD])    # 帧尾
    MAX_SERVO_COUNT = 9                  # 最大舵机数量
    SERVO_PINS = [2, 3, 4, 6, 44, 45, 46, 7, 10]  # 舵机引脚定义
    
    @staticmethod
    def calculate_checksum(data):
        """计算校验和 - 所有数据的和与上0xFF"""
        result = 0xAA + 0xBB  # 从帧头开始计算
        for byte in data:
            result += byte
        return result & 0xFF

    @staticmethod
    def angle_to_bytes(angle):
        """将角度值转换为两个字节（放大10倍后的uint16）"""
        value = int(angle * 10)
        return [(value >> 8) & 0xFF, value & 0xFF]  # 高字节在前，低字节在后

    def servo_control(self, servo_num, servo_id, servo_angle):
        """
        控制舵机的主函数
        
        参数:
        servo_num: 要控制的舵机数量 (int)
        servo_id: 舵机引脚号列表 (list[int])
        servo_angle: 对应舵机的角度列表 (list[float])
        
        返回:
        bytes: 完整的协议数据
        """
        # 参数检查
        if not 1 <= servo_num <= self.MAX_SERVO_COUNT:
            raise ValueError(f"舵机数量必须在1到{self.MAX_SERVO_COUNT}之间")
        if len(servo_id) != servo_num or len(servo_angle) != servo_num:
            raise ValueError("舵机ID列表和角度列表长度必须与舵机数量一致")
            
        # 构建舵机数据字典
        servo_data = {}
        for i in range(servo_num):
            pin = servo_id[i]
            if pin not in self.SERVO_PINS:
                raise ValueError(f"无效引脚号: {pin}，可用引脚: {self.SERVO_PINS}")
            servo_data[pin] = servo_angle[i]
            
        return self._generate_protocol(servo_data)

    def _generate_protocol(self, servo_data):
        """内部方法：根据舵机数据字典生成协议帧"""
        protocol_data = bytearray()
        protocol_data.extend(self.FRAME_HEADER)
        protocol_data.append(len(servo_data))
        
        # 填充舵机数据（按引脚顺序）
        for pin in self.SERVO_PINS:
            if pin in servo_data:
                protocol_data.append(pin)
                protocol_data.extend(self.angle_to_bytes(servo_data[pin]))
            else:
                protocol_data.extend([0x00, 0x00, 0x00])
        
        # 计算校验和
        protocol_data.append(self.calculate_checksum(protocol_data[2:]))
        protocol_data.extend(self.FRAME_TAIL)
        return bytes(protocol_data)


# # 示例用法
# if __name__ == "__main__":
#     protocol = ServoProtocol()
#     protocol.get_coordinate(50, 5, 140)
    
# #     # 直接控制3个舵机
# #     data = protocol.servo_control(
# #         servo_num=3,
# #         servo_id=[4, 6, 10],
# #         servo_angle=[90.0, 45.0, 180.0]
# #     )
    
# #     # 打印生成的协议数据（可选）
# #     print("生成的协议数据:", data.hex(' ').upper())
# #     total_bytes = len(data)
# #     print("字节:", total_bytes)